cmake_minimum_required(VERSION 3.14)  # 提升版本以支持现代 CMake 特性
project(navigation_exp_pkg)

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

# 查找 ROS 和 YAML-CPP 依赖
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  message_generation
  message_runtime
  roscpp
  rospy
  sensor_msgs
  std_msgs
  tf
  tf2_ros
  teb_local_planner
  costmap_2d
)

find_package(yaml-cpp REQUIRED)
find_package(Boost REQUIRED COMPONENTS graph system)
find_package(Eigen3 REQUIRED)

# 配置 catkin 包
catkin_package(
  CATKIN_DEPENDS
    geometry_msgs
    message_runtime
    roscpp
    rospy
    sensor_msgs
    std_msgs
    tf
    tf2_ros
    teb_local_planner
    costmap_2d
)

# 包含目录
include_directories(
  include
  ${catkin_INCLUDE_DIRS}
  ${YAML_CPP_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
  ${EIGEN3_INCLUDE_DIR}
  /usr/include/suitesparse
)


##############################################################
add_executable(graph_global_planner_node src/global_planner.cpp)

target_include_directories(graph_global_planner_node PRIVATE ${catkin_INCLUDE_DIRS})

target_link_libraries(
  graph_global_planner_node
  PRIVATE
  ${Boost_LIBRARIES}        
  ${catkin_LIBRARIES}       
)

######################################################
add_executable(local_planner_node src/local_planner.cpp)

target_include_directories(local_planner_node PRIVATE ${catkin_INCLUDE_DIRS})

target_link_libraries(local_planner_node
  PRIVATE
  ${catkin_LIBRARIES}
  ${YAML_CPP_LIBRARIES}
  ${Boost_LIBRARIES}
  ${EIGEN3_LIBRARIES}
  /opt/ros/noetic/lib/libcostmap_2d.so
  /opt/ros/noetic/lib/libteb_local_planner.so
)

########################################################
add_executable(navigation_simulate3d src/simulation_3d.cpp)

target_include_directories(navigation_simulate3d PRIVATE ${catkin_INCLUDE_DIRS})

target_link_libraries(navigation_simulate3d
  PRIVATE
  ${catkin_LIBRARIES}
  ${EIGEN3_LIBRARIES}
)

#########################################
# 添加python脚本
catkin_install_python(PROGRAMS
  scripts/cr_test.py
  scripts/navigation_simulate.py
  scripts/navigation_simulate3d.py
  scripts/cmd_controller.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)